/*
 * Copyright (c) 2020 Fraunhofer FOKUS and others. All rights reserved.
 *
 * See the NOTICE file(s) distributed with this work for additional
 * information regarding copyright ownership.
 *
 * This program and the accompanying materials are made available under the
 * terms of the Eclipse Public License 2.0 which is available at
 * http://www.eclipse.org/legal/epl-2.0
 *
 * SPDX-License-Identifier: EPL-2.0
 *
 * Contact: mosaic@fokus.fraunhofer.de
 */
package org.eclipse.mosaic.fed.carla.ambassador;

import org.apache.commons.lang3.StringUtils;
import org.eclipse.mosaic.fed.carla.clojure.ClojureCarlaConnection;
import org.eclipse.mosaic.fed.carla.clojure.complex.ClojureSimulationStepResult;
import org.eclipse.mosaic.fed.carla.config.CarlaConfiguration;
import org.eclipse.mosaic.interactions.application.CoSimulationStep;
import org.eclipse.mosaic.interactions.cooperation.CarlaConnect;
import org.eclipse.mosaic.interactions.traffic.TrafficLightUpdates;
import org.eclipse.mosaic.interactions.traffic.VehicleUpdates;
import org.eclipse.mosaic.interactions.vehicle.CarlaVehicleUpdates;
import org.eclipse.mosaic.lib.objects.trafficlight.TrafficLightGroup;
import org.eclipse.mosaic.lib.objects.trafficlight.TrafficLightGroupInfo;
import org.eclipse.mosaic.lib.objects.trafficlight.TrafficLightParam;
import org.eclipse.mosaic.lib.objects.trafficlight.TrafficLightState;
import org.eclipse.mosaic.lib.util.ProcessLoggingThread;
import org.eclipse.mosaic.lib.util.objects.ObjectInstantiation;
import org.eclipse.mosaic.rti.api.FederateExecutor;
import org.eclipse.mosaic.rti.api.Interaction;
import org.eclipse.mosaic.rti.api.InternalFederateException;
import org.eclipse.mosaic.rti.api.federatestarter.ExecutableFederateExecutor;
import org.eclipse.mosaic.rti.api.federatestarter.NopFederateExecutor;
import org.eclipse.mosaic.rti.api.parameters.AmbassadorParameter;
import org.eclipse.mosaic.rti.config.CLocalHost;

import javax.annotation.Nonnull;
import java.io.File;
import java.io.InputStream;
import java.util.*;
import java.util.stream.Collectors;

/**
 * Implementation of a {@link AbstractCarlaAmbassador} for the vehicle
 * simulator CARLA. It is used to visualize the traffic simulation in 3D
 * environment.
 *
 * Different from {@link CarlaAmbassador} directly control carla simulator
 * by python-clj
 */
public class ClojureCarlaAmbassador extends AbstractCarlaAmbassador{

    /**
     * Connection between CARLA federate and CARLA simulator.
     */
    private ClojureCarlaConnection carlaConnection = null;
    /**
     * Command used to start CARLA simulator.
     */
    private FederateExecutor federateExecutor = null;
    /**
     * CARLA configuration file
     */
    private CarlaConfiguration carlaConfig;
    /**
     * Carla simulator client port
     */
    private int carlaRpcPort = -1;
    /**
     * Next simulation step time.
     */
    private long nextTimeStep;

    /**
     * Creates a new {@link CarlaAmbassador} object.
     *
     * @param ambassadorParameter includes parameters for the CARLA Ambassador.
     */
    public ClojureCarlaAmbassador(AmbassadorParameter ambassadorParameter) {
        super(ambassadorParameter);
        try {
            // read the CARLA configuration file
            carlaConfig = new ObjectInstantiation<>(CarlaConfiguration.class, log)
                    .readFile(ambassadorParameter.configuration);
        } catch (InstantiationException e) {
            log.error("Configuration object could not be instantiated: ", e);
        }

        log.info("carlaConfig.updateInterval: " + carlaConfig.updateInterval);

        // check the carla configuration
        checkConfiguration();
    }

    /**
     * Check the updateInterval is validated.
     */
    private void checkConfiguration() {
        if (carlaConfig.updateInterval <= 0) {
            throw new RuntimeException("Invalid carla interval, should be >0");
        }

        if (StringUtils.isBlank(carlaConfig.carlaEggPath)) {
            throw new RuntimeException("Invalid carla egg path, should be not empty");
        }
    }

    /**
     * Creates and sets new federate executor.
     *
     * @param host name of the host (as specified in /etc/hosts.json)
     * @param port port number to be used by this federate
     * @param os   operating system enum
     * @return FederateExecutor.
     */
    @Nonnull
    @Override
    public FederateExecutor createFederateExecutor(String host, int port, CLocalHost.OperatingSystem os) {
        // CARLA needs to start the federate by itself, therefore we need to store the
        // federate starter locally and use it later
        federateExecutor = new ExecutableFederateExecutor(descriptor, getCarlaExecutable(carlaConfig, "CarlaUE4"),
                getProgramArguments(port));
        this.carlaRpcPort = port;
        // return federateExecutor
        return new NopFederateExecutor();
    }

    /**
     * This method is called to tell the federate the start time and the end time.
     * It is also used to start CARLA, and connect to CARLA.
     *
     * @param startTime Start time of the simulation run in nano seconds.
     * @param endTime   End time of the simulation run in nano seconds.
     * @throws InternalFederateException Exception is thrown if an error is occurred
     *                                   while execute of a federate.
     */
    @Override
    public void initialize(long startTime, long endTime) throws InternalFederateException {
        super.initialize(startTime, endTime);

        // wait for sumo to trigger

        startCarlaLocal();
    }

    /**
     * Connects to CARLA simulator using the given host and port.
     *
     * @param host host on which CARLA simulator is running.
     * @param port port on which CARLA client is listening.
     */
    @Override
    public void connectToFederate(String host, int port) {
        if (carlaConnection == null) {
            // start the carla connection
            carlaConnection = new ClojureCarlaConnection("localhost", carlaRpcPort, this, carlaConfig);
            carlaConnection.connect();
        }
    }

    @Override
    public void connectToFederate(String host, InputStream in, InputStream err) {
        this.connectToFederate(host, carlaRpcPort);
    }

    /**
     * Starts the CARLA binary locally.
     */
    void startCarlaLocal() throws InternalFederateException {
        if (!descriptor.isToStartAndStop()) {
            return;
        }

        File dir = new File(descriptor.getHost().workingDirectory, descriptor.getId());
        log.info("Start Federate local");
        log.info("Directory: " + dir);

        try {
            Process p = federateExecutor.startLocalFederate(dir);
            // read error output of process in an extra thread
            new ProcessLoggingThread(log, p.getInputStream(), "carla", ProcessLoggingThread.Level.Info).start();
            new ProcessLoggingThread(log, p.getErrorStream(), "carla", ProcessLoggingThread.Level.Error).start();

        } catch (FederateExecutor.FederateStarterException e) {
            log.error("Error while executing command: {}", federateExecutor.toString());
            throw new InternalFederateException("Error while starting Carla: " + e.getLocalizedMessage());
        }
    }

    /**
     * This method is called by the AbstractFederateAmbassador when a time advance
     * has been granted by the RTI. Before this call is placed, any unprocessed
     * interaction is forwarded to the federate using the processInteraction method.
     *
     * @param time The timestamp towards which the federate can advance it local
     *             time.
     */
    @Override
    public synchronized void processTimeAdvanceGrant(long time) throws InternalFederateException {
       // nop
    }

    /**
     * This method is called by the time management service to signal that the
     * simulation is finished.
     */
    @Override
    public void finishSimulation() throws InternalFederateException {
        log.info("Closing CARLA.");

        if (federateExecutor != null) {
            try {
                if (carlaConnection != null) {
                    carlaConnection.close();
                }
                federateExecutor.stopLocalFederate();
            } catch (FederateExecutor.FederateStarterException e) {
                log.warn("Could not properly stop federate");
            }
        }
    }

    @Override
    public boolean isTimeConstrained() {
        return false;
    }

    @Override
    public boolean isTimeRegulating() {
        return false;
    }


    @Override
    public synchronized void processInteraction(Interaction interaction) throws InternalFederateException {
        String type = interaction.getTypeId();
        long interactionTime = interaction.getTime();
        log.trace("Process interaction with type '{}' at time: {}", type, interactionTime);

        if (interaction.getTypeId().equals(CoSimulationStep.TYPE_ID)) {
            this.receiveInteraction((CoSimulationStep) interaction);
        } else if (interaction.getTypeId().equals(VehicleUpdates.TYPE_ID)) {
            this.receiveInteraction((VehicleUpdates) interaction);
        } else if (interaction.getTypeId().equals(CarlaConnect.TYPE_ID)) {
            this.receiveInteraction((CarlaConnect) interaction);
        } else if (interaction.getTypeId().equals(TrafficLightUpdates.TYPE_ID)) {
            this.receiveInteraction((TrafficLightUpdates) interaction);
        }
    }

    private void receiveInteraction(CoSimulationStep interaction) throws InternalFederateException {
        log.debug("Received CoSimulationStep: {}", interaction.getTime());

        long time = interaction.getTime();
        ClojureSimulationStepResult simulationStepResult = carlaConnection.getSimulationControl().simulateUntil(time, interaction);

        CarlaVehicleUpdates carlaVehicleUpdates = new CarlaVehicleUpdates(time, simulationStepResult.getSpawnedActors(), simulationStepResult.getRemovedActors(),
                simulationStepResult.getUpdateActors());

        // delegate SUMO ambassador
        try {
            rti.delegate("sumo", "synchronizeVehiclesFromCarla", new Class<?>[]{carlaVehicleUpdates.getClass()},
                    new Object[]{carlaVehicleUpdates});
        } catch (Exception ex) {
            throw new RuntimeException("Could not synchronize carla vehicles to sumo.", ex);
        }
    }

    private void receiveInteraction(VehicleUpdates interaction) throws InternalFederateException {
        log.debug("Received VehicleUpdates: {}", interaction.getTime());

        // sumo -> carla
        carlaConnection.getSimulationControl().synchronizeVehicle(interaction);
    }

    private void receiveInteraction(CarlaConnect interaction) {
        connectToFederate("localhost", carlaRpcPort);
    }

    private void receiveInteraction(TrafficLightUpdates interaction) throws InternalFederateException {
        List<String> sumoLandmarkIds = interaction.getTrafficLights().values()
                .stream()
                .flatMap(e -> e.getParams().values().stream())
                .flatMap(e -> e.getLandmark2Link().keySet().stream())
                .distinct()
                .collect(Collectors.toList());

        List<String> commonLandmarkId = sumoLandmarkIds.stream()
                .filter(e -> carlaConnection.tls.containsKey(e))
                .collect(Collectors.toList());

        Map<String, Object> sumoStateMap = new HashMap<>();

        Map<String, TrafficLightGroupInfo> trafficLightGroupInfos = interaction.getUpdated();
        for (String landmarkId : commonLandmarkId) {
            List<TrafficLightParam.TrafficLightLink> trafficLinks = getTrafficLinks(interaction, landmarkId);

            Set<Character> states = new HashSet<>();

            for (TrafficLightParam.TrafficLightLink trafficLink : trafficLinks) {
                TrafficLightState trafficLightState = trafficLightGroupInfos.get(trafficLink.getTrafficLightGroupId()).getCurrentState().get(trafficLink.getLinkIndex());
                states.add(trafficLightState.getCode());
            }

            if (states.size() > 1) {
                sumoStateMap.put(landmarkId, carlaConnection.getTrafficLightControl().getCarlaTrafficLightState('r'));
            } else if (states.size() == 1) {
                sumoStateMap.put(landmarkId, carlaConnection.getTrafficLightControl().getCarlaTrafficLightState(
                        states.stream().findFirst().orElseThrow(() -> new RuntimeException("Unexpected error"))
                ));
            } else {
                sumoStateMap.put(landmarkId, carlaConnection.getTrafficLightControl().getCarlaTrafficLightState('o'));
            }
        }


        for (String landmarkId : commonLandmarkId) {
            Object trafficLight = carlaConnection.tls.get(landmarkId);
            Object state = sumoStateMap.get(landmarkId);

            carlaConnection.getTrafficLightControl().setTrafficLightState(trafficLight, state);
        }
    }

    private List<TrafficLightParam.TrafficLightLink> getTrafficLinks(TrafficLightUpdates updates, String landmarkIds) {
        Collection<TrafficLightGroup> values = updates.getTrafficLights().values();
        Map<String, TrafficLightGroupInfo> trafficLightGroupInfoMap = updates.getUpdated();
        List<TrafficLightParam.TrafficLightLink> links = new ArrayList<>();

        for (TrafficLightGroup value : values) {
            String groupId = value.getGroupId();
            String currentProgramId = trafficLightGroupInfoMap.get(groupId).getCurrentProgramId();

            TrafficLightParam trafficLightParam = value.getParams().get(currentProgramId);

            links.addAll(trafficLightParam.getLandmark2Link().get(landmarkIds));
        }

        return links;
    }

}
